#include "appCar.h"
#include "Delay.h"
#include "oledDriver.h"
#include "Sw_comm_suport.h"
#include "tb6612.h"

mpu6050InfoStru mpu6050Info = {0};

static void SwPidAlgorithm()
{

}

/**
 * @brief 控制小车, 输入的参数是从遥控处发来的方向和速度信息
*/
static void SwDisplayMpu6050InfoToOled(mpu6050InfoStru *mpu6050Info)
{
	OLED_ShowSignedNum(2, 1, mpu6050Info->accX, 5);
    OLED_ShowSignedNum(3, 1, mpu6050Info->accY, 5);
    OLED_ShowSignedNum(4, 1, mpu6050Info->accZ, 5);
    OLED_ShowSignedNum(2, 8, mpu6050Info->groyX, 5);
    OLED_ShowSignedNum(3, 8, mpu6050Info->groyY, 5);
    OLED_ShowSignedNum(4, 8, mpu6050Info->groyZ, 5);
}

/**
 * @brief 控制小车, 输入的参数是从遥控处发来的方向和速度信息
*/
void ControlCar()
{
    // 获取MPU6050中的当前陀螺仪信息
    SwGetMpu6050Info(&mpu6050Info);
    // SwDisplayMpu6050InfoToOled(&mpu6050Info);
    
    // 根据陀螺仪信息进行PID计算
    SwPidAlgorithm();
    uint16_t speed = 80;
    // 根据计算结果控制马达
    Motor_SetLeftSpeed(speed);
    Motor_SetRightSpeed(speed);
}



void  SwGoFront()
{
    
}


void  SwGoBack()
{

}
void  SwGoLeft()
{

}
void  SwGoRight()
{
    
}
